# -*- coding: utf-8 -*-
# @copyright Copyright (c) 2022 OnMicro Corp.
# @brief     SNC Field Oriented Control algorithm test.
# @author    wei.lu@onmicro.com.cn
# @license   SPDX-License-Identifier: Apache-2.0

import wave
import pylab as plt
import matplotlib.patches as pat
import numpy as np
import scipy.stats as stats

# matlab: https://www.cnblogs.com/unclemac/p/12783347.html
#Serial = 0:0.1:100;
#Fs = 1;
#Phase = 0;
#Amp = 1;
#
#% 高频信号
#N0 = 2*pi*Fs*Serial - Phase;
#X0 = Amp*sin(N);
#subplot(4,1,1);
#plot(X0);
#
#% 低频信号
#Fs = 0.02;
#N1 = 2*pi*Fs*Serial - Phase;
#X1 = Amp*sin(N1);
#subplot(4,1,2);
#plot(X1);
#
#% 高频低频叠加的信号
#X2=X0+X1;
#subplot(4,1,3);
#plot(X2);
#
#%Xi-Yi=RC*(Yi - Yi-1)/DetalT
#len = length(X2);
#X3=X2;
#p=0.05;
#
#% 一阶RC滤波得到X3
#for i=2:len
#    X3(i) = p*X2(i)+(1-p)*X3(i-1);
#end
#
#subplot(4,1,4);
#plot(X3);

if __name__ == "__main__":
    Serial = np.arange(0, 1, 0.0001) #range=1sec step=0.1ms
    Phase = 0
    Amp = 1

    # 高频信号
    Fs = 100 #Hz
    N0 = 2*np.pi*Fs*Serial - Phase
    X0 = Amp*np.sin(N0)
    plt.figure()
    plt.plot(X0, label = 'hi freq 100Hz')

    # 低频信号
    Fs = 5
    N1 = 2*np.pi*Fs*Serial - Phase
    X1 = Amp*np.sin(N1)
    #plt.figure()
    plt.plot(X1, label = 'lo freq 5Hz')
    plt.legend()

    # 高频低频叠加的信号
    X2=X0+X1
    #plt.figure()
    plt.plot(X2, label = 'mix signal')
    
    # Xi-Yi=RC*(Yi - Yi-1)/DetalT
    X3=X2
    p=0.005
    # 一阶RC滤波得到X3
    for i in range(1, len(X2)):
        X3[i] = p*X2[i]+(1-p)*X3[i-1]
    #plt.plot(X3, label = 'p=0.05 filtered')

    # e(k) = e(k-1) + Ts/(RC+Ts) * (z(k) - e(k-1))
    X2=X0+X1
    X3=X2
    deltaT = 0.0001 #step
    fc = 20  #cut frequency
    RC = 1 / (2*np.pi*fc)
    K = deltaT / RC
    K = deltaT / (RC + deltaT)
    # 一阶RC滤波得到X3
    for i in range(1, len(X2)):
        X3[i] = X3[i-1] + K * (X2[i] - X3[i-1])
    plt.plot(X3, label = 'fc=20Hz')

    X2=X0+X1
    X3=X2
    deltaT = 0.0001
    fc = 5  #f_cut = f_lo
    RC = 1 / (2*np.pi*fc)
    K = deltaT / RC
    K = deltaT / (RC + deltaT)
    # 一阶RC滤波得到X3
    for i in range(1, len(X2)):
        X3[i] = X3[i-1] + K * (X2[i] - X3[i-1])
    plt.plot(X3, color='b', linewidth=2, label = 'fc=5Hz')
    # Show the phase delay is 45 deg when cutoff frequency equals the low part of input signals
    Amin = Amp
    xmin = 1
    for i in range(10, 1500):
        if (abs(X3[i]) < Amin):
            Amin = X3[i]
            xmin = i
    plt.axvline(xmin, color='b', linewidth=2)
    # the original low freq signal: 180 deg / 1000
    plt.axvline(1000, color='r', linewidth=1)

    # 再次应用一阶RC滤波得到X4
    X4=X3
    for i in range(1, len(X3)):
        X4[i] = X4[i-1] + K * (X3[i] - X4[i-1])
    plt.plot(X4, color='b', linewidth=1, label = '2nd LPF fc=5Hz')

    plt.title('1-order RC LPF')
    plt.xlabel('Time (0.1ms)')
    plt.grid()
    plt.legend()
    plt.show()
